//
// Created by LiuDongPeng on 2023/11/16.
//

#include "foc.h"


/**
 * @brief Clark transform
 * @param[in]   ia
 * @param[in]   ib
 * @param[in]   ic
 * @param[out]  ialpha
 * @param[out]  ibeta
 */
void foc_clark(float32_t ia, float32_t ib, float32_t ic, float32_t *ialpha, float32_t *ibeta)
{
    if (ialpha == NULL || ibeta == NULL)
        return;

//    *ialpha = (ia * 2 - (ib + ic)) / 3.0f;
//    *ibeta = (ib - ic) * SQRT_3 / 3.0f;

    arm_clarke_f32(ia, ib, ialpha, ibeta);
}


/**
 * @brief Park transform
 * @param[in]   ialpha
 * @param[in]   ibeta
 * @param[in]   sinTheta
 * @param[in]   cosTheta
 * @param[out]  id
 * @param[out]  iq
 */
void foc_park(float32_t ialpha, float32_t ibeta, float32_t sinTheta, float32_t cosTheta, float32_t *id, float32_t *iq)
{
    if (id == NULL || iq == NULL)
        return;

//    *id = ialpha * cosTheta + ibeta * sinTheta;
//    *iq = -ialpha * sinTheta + ibeta * cosTheta;

    arm_park_f32(ialpha, ibeta, id, iq, sinTheta, cosTheta);
}

/**
 * @brief Inv park transform
 * @param[in]   ud
 * @param[in]   uq
 * @param[in]   sinTheta
 * @param[in]   cosTheta
 * @param[out]  ualpha
 * @param[out]  ubeta
 */
void foc_inv_park(float32_t ud, float32_t uq, float32_t sinTheta, float32_t cosTheta, float32_t *ualpha, float32_t *ubeta)
{
    if (ualpha == NULL || ubeta == NULL)
        return;

    *ualpha = ud * cosTheta - uq * sinTheta;
    *ubeta = ud * sinTheta + uq * cosTheta;
}


/**
 * @brief Calc 3 phase pwm duty
 * @param[in]   ualpha
 * @param[in]   ubeta
 * @param[in]   udc
 * @param[in]   tpwm
 * @param[out]  ta
 * @param[out]  tb
 * @param[out]  tc
 */
void foc_svpwm(float32_t ualpha, float32_t ubeta, float32_t udc, float32_t tpwm,
               float32_t *ta, float32_t *tb, float32_t *tc)
{
    if (ta == NULL || tb == NULL || tc == NULL)
        return;

    // 0. 预备工作
    const float32_t sqrt3 = SQRT_3;
    const float32_t sqrt3TpwmUdc = sqrt3 * tpwm / udc;

    /* 1. 计算扇区 */
    const int sectorTable[8] = {0, 2, 6, 1, 4, 3, 5, 0};
    int sector;
    int A, B, C, N;
    float32_t u1, u2, u3;

    u1 = ubeta;
    u2 = (sqrt3 * ualpha - ubeta) * 0.5f;
    u3 = (-sqrt3 * ualpha - ubeta) * 0.5f;

    A = u1 > 0 ? 1 : 0;
    B = u2 > 0 ? 1 : 0;
    C = u3 > 0 ? 1 : 0;
    N = 4 * C + 2 * B + A;
    sector = sectorTable[N];


    // 2. 根据扇区计算Tx Ty
    float32_t Tx, Ty;
    switch (sector)
    {
        case 1:
            Tx = sqrt3TpwmUdc * u2;
            Ty = sqrt3TpwmUdc * u1;
            break;

        case 2:
            Tx = sqrt3TpwmUdc * -u2;
            Ty = sqrt3TpwmUdc * -u3;
            break;

        case 3:
            Tx = sqrt3TpwmUdc * u1;
            Ty = sqrt3TpwmUdc * u3;
            break;

        case 4:
            Tx = sqrt3TpwmUdc * -u1;
            Ty = sqrt3TpwmUdc * -u2;
            break;

        case 5:
            Tx = sqrt3TpwmUdc * u3;
            Ty = sqrt3TpwmUdc * u2;
            break;

        case 6:
        default:
            Tx = sqrt3TpwmUdc * -u3;
            Ty = sqrt3TpwmUdc * -u1;
            break;
    }

    float32_t Tsum = Tx + Ty;
    if (Tsum > tpwm)
    {
        Tx = Tx / Tsum * tpwm;
        Ty = Ty / Tsum * tpwm;
    }

    // 3. 根据扇区计算三路PWM占空比
    float32_t Sa, Sb, Sc;

    float32_t val1 = (tpwm - Tx - Ty) * 0.25f;
    float32_t val2 = val1 + Tx * 0.5f;
    float32_t val3 = val2 + Ty * 0.5f;

    switch (sector)
    {
        case 1:
            Sa = val1;
            Sb = val2;
            Sc = val3;
            break;

        case 2:
            Sa = val2;
            Sb = val1;
            Sc = val3;
            break;

        case 3:
            Sa = val3;
            Sb = val1;
            Sc = val2;
            break;

        case 4:
            Sa = val3;
            Sb = val2;
            Sc = val1;
            break;

        case 5:
            Sa = val2;
            Sb = val3;
            Sc = val1;
            break;

        case 6:
        default:
            Sa = val1;
            Sb = val3;
            Sc = val2;
            break;
    }

    *ta = Sa;
    *tb = Sb;
    *tc = Sc;
}
